#include <ros/ros.h>

#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

std::string file_directory;
std::string file_name;
std::string pcd_file;

std::string map_topic_name;

const std::string pcd_format = ".pcd";

nav_msgs::OccupancyGrid map_topic_msg;

double map_resolution = 0.05;
double obstacle_min_z;
double obstacle_max_z;
double ground_threshold;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud(new pcl::PointCloud<pcl::PointXYZ>);

void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::OccupancyGrid& msg,
                    double obstacle_min_z,
                    double obstacle_max_z,
                    double ground_threshold);

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pcl_filters");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    // 先读取参数，顺序不能错！
    private_nh.param("file_directory", file_directory, std::string("/home/unitree/map_fcy/pcd2pgm_ws/src/pcd2pgm/pcd"));
    private_nh.param("file_name", file_name, std::string("fuzhou"));
    private_nh.param("map_resolution", map_resolution, 0.05);
    private_nh.param("map_topic_name", map_topic_name, std::string("pcd_map"));

    private_nh.param("obstacle_min_z", obstacle_min_z, 0.1);
    private_nh.param("obstacle_max_z", obstacle_max_z, 2.0);
    private_nh.param("ground_threshold", ground_threshold, 0.05);

    ROS_INFO("*** file_directory = %s ***", file_directory.c_str());
    ROS_INFO("*** file_name = %s ***", file_name.c_str());
    ROS_INFO("Obstacle min Z: %f", obstacle_min_z);
    ROS_INFO("Obstacle max Z: %f", obstacle_max_z);
    ROS_INFO("Ground threshold: %f", ground_threshold);
    
    // 先拼路径
    std::string pcd_file = file_directory + file_name + ".pcd";
    
    ROS_INFO("*** pcd_file = %s ***", pcd_file.c_str());
    
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file, *pcd_cloud) == -1)
    {
        PCL_ERROR("Couldn't read file: %s \n", pcd_file.c_str());
        return -1;
    }
    
    std::cout << "原始点云点数：" << pcd_cloud->points.size() << std::endl;
    
    // 生成时间戳
    time_t raw_time = static_cast<time_t>(ros::Time::now().sec);
    struct tm* time_info = localtime(&raw_time);
    std::stringstream ts_ss;
    ts_ss << "_" << time_info->tm_hour << "_" << time_info->tm_min;
    std::string timestamp = ts_ss.str();
    
    // 输出地图路径
    std::string map_pgm_file  = file_directory + file_name + timestamp + ".pgm";
    std::string map_yaml_file = file_directory + file_name + timestamp + ".yaml";
    ROS_INFO("*** Map 文件: %s / %s ***", map_pgm_file.c_str(), map_yaml_file.c_str());


    ros::Publisher map_topic_pub = nh.advertise<nav_msgs::OccupancyGrid>(map_topic_name, 1);

    if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file, *pcd_cloud) == -1)
    {
        PCL_ERROR("Couldn't read file: %s \n", pcd_file.c_str());
        return -1;
    }
    
    // 点云降采样（体素滤波）
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(pcd_cloud);
    vg.setLeafSize(0.05f, 0.05f, 0.05f);
    vg.filter(*cloud_filtered);

    std::cout << "降采样后点数：" << cloud_filtered->points.size() << std::endl;

    // 转成OccupancyGrid，传参数进去
    SetMapTopicMsg(cloud_filtered, map_topic_msg, obstacle_min_z, obstacle_max_z, ground_threshold);

    ros::Rate loop_rate(1.0);
    while (ros::ok())
    {
        map_topic_pub.publish(map_topic_msg);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::OccupancyGrid& msg,
                    double obstacle_min_z,
                    double obstacle_max_z,
                    double ground_threshold)
{
    msg.header.seq = 0;
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = "map";
    msg.info.map_load_time = ros::Time::now();
    msg.info.resolution = map_resolution;

    if (cloud->points.empty())
    {
        ROS_WARN("pcd is empty!");
        return;
    }

    double x_min, x_max, y_min, y_max;
    x_min = x_max = cloud->points[0].x;
    y_min = y_max = cloud->points[0].y;

    for (const auto& pt : cloud->points)
    {
        if (pt.x < x_min) x_min = pt.x;
        if (pt.x > x_max) x_max = pt.x;
        if (pt.y < y_min) y_min = pt.y;
        if (pt.y > y_max) y_max = pt.y;
    }

    msg.info.origin.position.x = x_min;
    msg.info.origin.position.y = y_min;
    msg.info.origin.position.z = 0.0;
    msg.info.origin.orientation.x = msg.info.origin.orientation.y = msg.info.origin.orientation.z = 0.0;
    msg.info.origin.orientation.w = 1.0;

    msg.info.width  = int((x_max - x_min) / map_resolution) + 1;
    msg.info.height = int((y_max - y_min) / map_resolution) + 1;
    msg.data.assign(msg.info.width * msg.info.height, -1);
    
    ROS_INFO("地图尺寸: %d x %d, 总栅格数: %lu", msg.info.width, msg.info.height, msg.data.size());

    // 记录每个栅格最大高度
    std::vector<double> max_z(msg.info.width * msg.info.height, -1e9);
    for (const auto& pt : cloud->points)
    {
        if (pt.z < obstacle_min_z || pt.z > obstacle_max_z)
            continue;

        int i = int((pt.x - x_min) / map_resolution);
        int j = int((pt.y - y_min) / map_resolution);
        if (i < 0 || i >= msg.info.width || j < 0 || j >= msg.info.height)
            continue;

        int idx = i + j * msg.info.width;
        if (pt.z > max_z[idx])
            max_z[idx] = pt.z;
    }

    // 填充OccupancyGrid
    for (size_t idx = 0; idx < max_z.size(); ++idx)
    {
        if (max_z[idx] < ground_threshold)
            msg.data[idx] = 0;          // 空闲
        else if (max_z[idx] >= obstacle_min_z)
            msg.data[idx] = 100;        // 障碍
        else
            msg.data[idx] = -1;         // 未知
    }
}

